/*
 www.pipoisu.com
 Copyright (c) 2011.  All rights reserved.

 File     : mixer.h
 Author(s): Erez Raviv

 This program is free software: you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 (at your option) any later version.

 This program is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 GNU General Public License for more details.

 You should have received a copy of the GNU General Public License
 along with this program. If not, see <http://www.gnu.org/licenses/>.
*/


#ifndef MIXER_H
#define MIXER_H

#include "pipo.h"

#define THR_CHN			3	//(2-(pipoData.stickMode&1))
#define PITCH_CHN		2	//(1+(pipoData.stickMode&1))
#define ROLL_CHN		1	//((pipoData.stickMode&2) ? 0 : 3)
#define YAW_CHN			4	//((pipoData.stickMode&2) ? 3 : 0)

#define SWASH_TYPE_120   1
#define SWASH_TYPE_120X  2
#define SWASH_TYPE_140   3
#define SWASH_TYPE_90    4
#define SWASH_TYPE_NUM   4

#define MIX_P1    5
#define MIX_P2    6
#define MIX_P3    7
#define MIX_MAX   8
#define MIX_FULL  9
#define MIX_CYC1  10
#define MIX_CYC2  11
#define MIX_CYC3  12

#define DR_HIGH   0
#define DR_MID    1
#define DR_LOW    2
#define DR_EXPO   0
#define DR_WEIGHT 1
#define DR_RIGHT  0
#define DR_LEFT   1
#define DR_DRSW1  99
#define DR_DRSW2  98

#define SW_BASE      SW_ThrCt

#define CS_OFF       0
#define CS_VPOS      1  //v>offset
#define CS_VNEG      2  //v<offset
#define CS_APOS      3  //|v|>offset
#define CS_ANEG      4  //|v|<offset
#define CS_AND       5
#define CS_OR        6
#define CS_XOR       7
#define CS_EQUAL     8
#define CS_NEQUAL    9
#define CS_GREATER   10
#define CS_LESS      11
#define CS_EGREATER  12
#define CS_ELESS     13
#define CS_MAXF      13  //max function

#define CS_VOFS       0
#define CS_VBOOL      1
#define CS_VCOMP      2
#define CS_STATE(x)   ((x)<CS_AND ? CS_VOFS : ((x)<CS_EQUAL ? CS_VBOOL : CS_VCOMP))


#define MAX_DRSWITCH (1+SW_Trainer-SW_ThrCt+1+NUM_CSW)

#define GET_DR_STATE(x) (!getSwitch(pipoData.expoData[x].drSw1,0,0) ?   \
    DR_HIGH :                                  \
    !getSwitch(pipoData.expoData[x].drSw2,0,0)?   \
    DR_MID : DR_LOW);

// set motors to corresponding output channels
#define MOTOR_FR 8
#define MOTOR_BR 7
#define MOTOR_BL 6
#define MOTOR_FL 5

#define MOTOR_IDLE (-800) // PWM command value for motor idle

#define maxCmdRadPerSec 4 // this is max commandable rotation velocity in rad/s 

extern inline int16_t calc100toRESX(int8_t x)
{
  return ((x*41)>>2) - x/64;
}

inline int16_t calc1000toRESX(int16_t x)  // improve calc time by Pat MacKenzie
{
    int16_t y = x>>5;
    x+=y;
    y=y>>2;
    x-=y;
    return x+(y>>2);
//  return x + x/32 - x/128 + x/512;
}

enum EnumKeys {
  KEY_MENU ,
  KEY_EXIT ,
  KEY_DOWN ,
  KEY_UP  ,
  KEY_RIGHT ,
  KEY_LEFT ,
  TRM_LH_DWN  ,
  TRM_LH_UP   ,
  TRM_LV_DWN  ,
  TRM_LV_UP   ,
  TRM_RV_DWN  ,
  TRM_RV_UP   ,
  TRM_RH_DWN  ,
  TRM_RH_UP   ,
  //SW_NC     ,
  //SW_ON     ,
  SW_ThrCt  ,
  SW_RuddDR ,
  SW_ElevDR ,
  SW_ID0    ,
  SW_ID1    ,
  SW_ID2    ,
  SW_AileDR ,
  SW_Gear   ,
  SW_Trainer
};

void mixer();

#endif // MIXER_H
